#pragma once
#include "can.hpp"
#include <cstring>
#include <map>
#include <thread>
#include <functional>

// work on another thread to deal with can frams
class CanServer
{
  public:
    // todo startRecThread must be true current
    bool StartServer(std::string dev, bool startRecThread = true);
    template <size_t n> bool Send(uint32_t id, const std::array<unsigned char, n> &bytes);
    template <typename T> bool Send(uint32_t id, T data);
    template <typename U, typename T> bool Send(uint32_t id, U data1, T data2);
    void RegeistCallBack(uint32_t id, std::function<void(const can_frame &)> callback);
    void RemoveCallBack(uint32_t id);
    void Step();
    ~CanServer();

  private:
    std::map<uint32_t, std::function<void(const can_frame &)>> callbacks;
    std::thread recThread;
    Can can;
    bool exitFlag;
    void Loop();
};

template <size_t n> bool CanServer::Send(uint32_t id, const std::array<unsigned char, n> &bytes)
{
    static_assert(n <= 8);
    can_frame frame{.can_id = id, .can_dlc = n};
    copy(bytes.begin(), bytes.end(), std::begin(frame.data));
    return can.SendFrame(frame);
}

template <typename T> bool CanServer::Send(uint32_t id, T data)
{
    static_assert(sizeof(T) <= 8);
    can_frame frame{.can_id = id, .can_dlc = sizeof(T)};
    std::memcpy(frame.data, &data, sizeof(T));
    std::reverse(std::begin(frame.data), std::begin(frame.data) + sizeof(T)); // due to byte sequence
    return can.SendFrame(frame);
}

template <typename U, typename T> bool CanServer::Send(uint32_t id, U data1, T data2)
{
    constexpr size_t len1 = sizeof(U);
    constexpr size_t len2 = sizeof(T);
    constexpr size_t total = len1 + len2;
    static_assert(total <= 8);
    can_frame frame{.can_id = id, .can_dlc = total};
    std::memcpy(frame.data, &data1, len1);
    std::reverse(std::begin(frame.data), std::begin(frame.data) + len1); // due to byte sequence
    std::memcpy(frame.data + len1, &data2, len2);
    std::reverse(std::begin(frame.data) + len1, std::begin(frame.data) + total); // due to byte sequence
    return can.SendFrame(frame);
}